// Made for ATtiny2313


// Motordriver PINs
int rdir = 3; // PIN for right wheel direction
boolean rdir_v = false; // Value for ridr - True = Reverse, False = Forward
int ldir = 4; // PIN for left wheel direction
boolean ldir_v = false; // Value for ldir - True = Forward, False = Rewind
int rspeedpin = 5; // PIN for right wheel speed
int lspeedpin = 6; // PIN for left wheel speed
// Motorstates
boolean motorrun = false; // status for the motor - running or not
int initialspeed = 100; // Value for the initial speed for rspeed and lspeed
int rcurr_speed = 0; // Value for the current speed of the right wheel
int lcurr_speed = 0; // Value for the current speed of the left wheel

// Distance Pins
//int front_pin = 10; // PIN for the front-sensor
//int back_pin = 11; // PIN for the back-sensor
//int right_pin = 12; // PIN for the right-sensor
//int left_pin = 13; // PIN for the left-sensor
// Distance States
//int front_dist = 0; // Value for the current distance - front
//int back_dist = 0; // Value for the current distance - back
//int right_dist = 0; // Value for the current distance - right
//int left_dist = 0; // Value for the current distance - left

void setup()
{
  Serial.begin(9600);
  pinMode(rdir, OUTPUT);
  pinMode(ldir, OUTPUT);
  pinMode(rspeedpin, OUTPUT);
  pinMode(lspeedpin, OUTPUT);
  pinMode(10, INPUT);
  pinMode(11, INPUT);
  pinMode(12, INPUT);
  pinMode(13, INPUT);
}
void loop()
{
  int xcommand = 0;
  int query_distance = 0;
  int query_pin = 0;
  boolean distance_measure = false;
  
  boolean motordrive = false;
  
  if (Serial.available()){
    xcommand = Serial.read();
  }
  
  switch (xcommand) {
    case 1: //forward
      rdir_v = true;
      ldir_v = true;
      lcurr_speed = initialspeed;
      rcurr_speed = initialspeed;
      //motorset();
      motorrun = true;
      break;
    
    case 2: //rewind
      rdir_v = false;
      ldir_v = false;
      //motorset();
      motordrive = true;
      break;
    
    case 3: //right
      lcurr_speed - 10;
      //motorset();
      break;
    
    case 4: //sharp right
      rcurr_speed - rcurr_speed;
      //motorset();
      motordrive = true;
      break;

    case 5: // left
      rcurr_speed - 10;
      //motorset();
      motordrive = true;
      break;
      
    case 6: // sharp left
      lcurr_speed - lcurr_speed;
      //motorset();
      motordrive = true;
      break;
    
    case 7: // faster both wheels
      lcurr_speed + 10;
      rcurr_speed + 10;
      //motorset();
      motordrive = true;
      break;
    
    case 8: // slower both wheels
      lcurr_speed - 10;
      rcurr_speed - 10;
      //motorset();
      motordrive = true;
      break;

    case 9: // STOP
      rcurr_speed = 0;
      lcurr_speed = 0;
      //motorset();
      motordrive = true;
      break;
    
    case 11: // Front distance measure
      //query_distance = analogRead(front_pin);
      query_pin = 10;
      distance_measure = true;
      break;
      
    case 12: // Back distance measure
      //query_distance = analogRead(back_pin);
      query_pin = 11;
      distance_measure = true;
      break;
      
    case 13: // Right distance measure
      //query_distance = analogRead(right_pin);
      query_pin = 12;
      distance_measure = true;
      break;
      
    case 14: // Left distance measure
      //query_distance = analogRead(left_pin);
      query_pin = 13;
      distance_measure = true;

  }
  
  if (distance_measure == true){
    query_distance = analogRead(query_pin);
  }
  
  if (motordrive == true){
    if (rdir_v == true){
      digitalWrite(rdir, HIGH);
    }
    else{
      digitalWrite(rdir, LOW);
    }
    
    if (ldir_v == true){
      digitalWrite(ldir, HIGH);
    }
    else{
      digitalWrite(rdir, LOW);
    }
    
    analogWrite(rspeedpin, rcurr_speed);
    analogWrite(lspeedpin, lcurr_speed);
    
    motordrive = false;
  }
}

/*void motorset(){
  rdir_set();
  ldir_set();
  rspeed_set();
  lspeed_set();
}
    
void rdir_set(){
  if (rdir_v == true){
    digitalWrite(rdir, HIGH);
  }
  else{
    digitalWrite(rdir, LOW);
  }
}

void ldir_set(){
  if (ldir_v == true){
    digitalWrite(ldir, HIGH);
  }
  else{
    digitalWrite(rdir, LOW);
  }
}

void rspeed_set(){
  analogWrite(rspeedpin, rcurr_speed);
}

void lspeed_set(){
  analogWrite(lspeedpin, lcurr_speed);
}*/


